
#include "tf_Common.h"

#include "tf_Coordinate.h"
#include "tf_Geom_Algorithm.h"

using namespace std;



TfCoordinate::TfCoordinate()
{
	memset(m_o,0,sizeof(double)*3);
	memset(m_u,0,sizeof(double)*3);
	memset(m_v,0,sizeof(double)*3);
	memset(m_w,0,sizeof(double)*3);
	m_u[2] = m_v[2] = m_w[2] = 1.0;
}

TfCoordinate::TfCoordinate(double o[3],double u[3],double v[3],double w[3])
{
	UnitVector(u);
	UnitVector(v);
	UnitVector(w);
	memcpy(this->m_o,o,sizeof(double)*3);
	memcpy(this->m_o,u,sizeof(double)*3);
	memcpy(this->m_o,v,sizeof(double)*3);
	memcpy(this->m_o,w,sizeof(double)*3);
}

TfCoordinate::~TfCoordinate()
{

}
void TfCoordinate::CalculateFeatureMatrix(double matrix[4][4])
{
	InitMatrix(matrix);
	matrix[0][0] = m_u[0];matrix[0][1] = m_v[0];matrix[0][2] = m_w[0];
	matrix[1][0] = m_u[1];matrix[1][1] = m_v[1];matrix[1][2] = m_w[1];
	matrix[2][0] = m_u[2];matrix[2][1] = m_v[2];matrix[2][2] = m_w[2];
	matrix[3][0] = -(m_o[0]*m_u[0]+m_o[1]*m_u[1]+m_o[2]*m_u[2]);
	matrix[3][1] = -(m_o[0]*m_v[0]+m_o[1]*m_v[1]+m_o[2]*m_v[2]);
	matrix[3][2] = -(m_o[0]*m_w[0]+m_o[1]*m_w[1]+m_o[2]*m_w[2]);
}

void TfCoordinate::GlobleToLocal(double ptGloble[3],double ptLocal[3])
{
	double matrix[4][4] = {0.0};
	CalculateFeatureMatrix(matrix);
	PointPlusMatrix(ptGloble,matrix,ptLocal);

}

void TfCoordinate::LocalToGloble(double ptLocal[3],double ptGloble[3])
{
	double matrix[4][4] = {0.0};
	CalculateFeatureMatrix(matrix);
	InverseMatrix(matrix[0],4);
	PointPlusMatrix(ptLocal,matrix,ptGloble);
}